20. Particle Weights Solution
We can calculate particle weights using the following equation:
- Recall that for this example the standard deviation for both x and y is 0.3.
- x and y are the observations in map coordinates from landmarks quiz and \mu_x , \mu_y are the coordinates of the nearest landmarks. These should correspond to the correct responses from previous quizzes.
Quiz Solutions
OBS1 weight should be around
0.00683644777551
rounding to
6.84E-3
.
OBS2 weight should be around
0.00683644777551
, rounding to
6.84E-3
.
OBS3 weight should be around
9.83184874151e-49
, rounding to
9.83E-49
.
The final weight should be around
4.59112934464959e-53
, rounding to
4.60E-53
(the above three multiplied together).
We have included a C++ solution below; the
multiv_gauss.cpp
file contains the primary calculations.
Start Quiz:
#include <iostream>
#include "multiv_gauss.h"
int main() {
// define inputs
double sig_x, sig_y, x_obs, y_obs, mu_x, mu_y;
// define outputs for observations
double weight1, weight2, weight3;
// final weight
double final_weight;
// OBS1 values
sig_x = 0.3;
sig_y = 0.3;
x_obs = 6;
y_obs = 3;
mu_x = 5;
mu_y = 3;
// Calculate OBS1 weight
weight1 = multiv_prob(sig_x, sig_y, x_obs, y_obs, mu_x, mu_y);
// should be around 0.00683644777551 rounding to 6.84E-3
std::cout << "Weight1: " << weight1 << std::endl;
// OBS2 values
sig_x = 0.3;
sig_y = 0.3;
x_obs = 2;
y_obs = 2;
mu_x = 2;
mu_y = 1;
// Calculate OBS2 weight
weight2 = multiv_prob(sig_x, sig_y, x_obs, y_obs, mu_x, mu_y);
// should be around 0.00683644777551 rounding to 6.84E-3
std::cout << "Weight2: " << weight2 << std::endl;
// OBS3 values
sig_x = 0.3;
sig_y = 0.3;
x_obs = 0;
y_obs = 5;
mu_x = 2;
mu_y = 1;
// Calculate OBS3 weight
weight3 = multiv_prob(sig_x, sig_y, x_obs, y_obs, mu_x, mu_y);
// should be around 9.83184874151e-49 rounding to 9.83E-49
std::cout << "Weight3: " << weight3 << std::endl;
// Output final weight
final_weight = weight1 * weight2 * weight3;
// 4.60E-53
std::cout << "Final weight: " << final_weight << std::endl;
return 0;
}
#include "multiv_gauss.h"
#include <cmath>
double multiv_prob(double sig_x, double sig_y, double x_obs, double y_obs,
double mu_x, double mu_y) {
// calculate normalization term
double gauss_norm;
gauss_norm = 1 / (2 * M_PI * sig_x * sig_y);
// calculate exponent
double exponent;
exponent = (pow(x_obs - mu_x, 2) / (2 * pow(sig_x, 2)))
+ (pow(y_obs - mu_y, 2) / (2 * pow(sig_y, 2)));
// calculate weight using normalization terms and exponent
double weight;
weight = gauss_norm * exp(-exponent);
return weight;
}
#ifndef MULTIV_GAUSS_H
#define MULTIV_GAUSS_H
double multiv_prob(double sig_x, double sig_y, double x_obs, double y_obs,
double mu_x, double mu_y);
#endif // MULTIV_GAUSS_H